#include "cart_model.h"
#include <iostream>

const float CartModel::cartWidth  = 0.2f;
const float CartModel::cartHeight = 0.2f;
const float CartModel::cartDepth = 0.2f;

const float CartModel::lightPos[] = {0.0f,0.94f,0.0f,1.0f};
const float CartModel::lightColor[] = {1.0f,1.0f,0.0f,1.0f};

const Point CartModel::points[]={	Point(0.8f,-0.8f,-0.5f),
									Point(0.8f,0.8f,-0.5f),
									Point(-0.8f,0.8f,-0.5f),
									Point(-0.8f,-0.8f,-0.5),
									Point(-1.0f,1.0f,0.5f),
									Point(-1.0f,-1.0f,0.5f),
									Point(1.0f,-1.0f,0.5f),
									Point(1.0f,1.0f,0.5f)};

const unsigned int CartModel::indices[] = { 0,1,3,2,5,4,2,7,1,6,0,5,3 };

CartModel::CartModel(unsigned int lightId){
	this->pos = Point(0,0,0);
	this->xytheta = 0;
	this->yztheta =0;
	this->lightState = true;
	this->dir = Point(0,1,0);
	this->lightId = lightId;
}

CartModel::~CartModel(){}

void CartModel::updatePosition(const PathIterator& it){
	this->pos=it.getCurrent();
	Point last = it.getLast();
	this->dir = last-this->pos;
	float m = sqrt(pow(dir.x1,2)+pow(dir.x2,2));
	this->xytheta = atan2(dir.x3,m);
	this->yztheta = atan2(dir.x1,dir.x2);
}

const float* CartModel::getLightPosition(){
	static float lpos[4] = {0,0,0,1.0f};
	lpos[0] = this->pos.x1+CartModel::lightPos[0];
	lpos[1] = this->pos.x2+CartModel::lightPos[1];
	lpos[2] = this->pos.x3+CartModel::lightPos[2];
	return lpos;
}
